#!/usr/bin/env python3
# coding: utf-8

import os
import threading
import time
import yaml

import rospy
from std_msgs.msg import String
from std_msgs.msg import Bool
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Twist
from actionlib_msgs.msg import GoalStatusArray
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal


class SequentialNavigator(object):
	def __init__(self):
		rospy.init_node('sequential_nav_detect', anonymous=False)

		# Params
		self.frame_id = rospy.get_param('~frame_id', 'map')
		# Accept multiple possible param names for the yaml path
		self.goals_yaml = rospy.get_param('~goals_yaml', None)
		if self.goals_yaml is None:
			self.goals_yaml = rospy.get_param('~goals_yaml_path', None)
		if self.goals_yaml is None:
			self.goals_yaml = rospy.get_param('~nav_goal_yaml', None)
		if not self.goals_yaml:
			rospy.logerr('未提供~goals_yaml参数（nav_goal.yaml路径）。请通过launch传入。')
			raise rospy.ROSInitException('goals_yaml 参数缺失')

		if not os.path.isabs(self.goals_yaml):
			self.goals_yaml = os.path.abspath(self.goals_yaml)

		if not os.path.exists(self.goals_yaml):
			rospy.logerr('goals_yaml 文件不存在: {}'.format(self.goals_yaml))
			raise rospy.ROSInitException('goals_yaml 文件不存在')

		# Timeouts (seconds)
		self.nav_timeout_sec = float(rospy.get_param('~nav_timeout', 300.0))
		self.detect_timeout_sec = float(rospy.get_param('~detect_timeout', 120.0))

		# Action client for move_base
		self.mb_client = actionlib.SimpleActionClient('/move_base', MoveBaseAction)
		if not self.mb_client.wait_for_server(rospy.Duration(5.0)):
			rospy.logerr('等待 /move_base ActionServer 超时，请确认 move_base 已启动')
			raise rospy.ROSInitException('move_base action server 不可用')

		# Publishers
		self.goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=10, latch=True)
		self.yolo_pub = rospy.Publisher('/yolo_detect', String, queue_size=10)
		self.start_nav_pub = rospy.Publisher('/nx_nano/start_nav', Bool, queue_size=10, latch=True)
		self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

		# Internal state
		self._last_goal_sent_time = rospy.Time(0)
		self._nav_succeeded = threading.Event()
		self._detected = threading.Event()

		# Subscribers
		self.status_sub = rospy.Subscriber('/move_base/status', GoalStatusArray, self._on_status)
		# Primary topic per requirement
		self.detect_sub = rospy.Subscriber('/dog_action_statet', String, self._on_detect)
		# Backward compatibility with existing controller that publishes '/dog_action_state'
		self.detect_sub_compat = rospy.Subscriber('/dog_action_state', String, self._on_detect)

		# Load goals
		self.goals = self._load_goals(self.goals_yaml)
		if not self.goals:
			rospy.logerr('未能从 {} 读取到 one..six 的目标点'.format(self.goals_yaml))
			raise rospy.ROSInitException('未找到有效目标点')

		# Optional extra goal: navigate to 'box1' after 'six' without triggering detection
		self.box1_goal = self._load_named_goal(self.goals_yaml, 'box1')
		# Optional intermediate goals: ball1..ball4 between 'six' and 'box1'
		self.ball_goals = self._load_ball_goals(self.goals_yaml)

		rospy.loginfo('顺序导航节点启动，共 {} 个目标点'.format(len(self.goals)))

	def _load_goals(self, yaml_path):
		with open(yaml_path, 'r', encoding='utf-8') as f:
			data = yaml.safe_load(f) or {}
		goals_ns = data.get('goals', {})
		sequence_keys = ['one', 'two', 'three', 'four', 'five', 'six']
		seq = []
		for key in sequence_keys:
			g = goals_ns.get(key)
			if not g:
				rospy.logwarn('在 {} 中未找到目标: {}'.format(yaml_path, key))
				continue
			pos = g.get('position', None)
			ori = g.get('orientation', None)
			if not (isinstance(pos, (list, tuple)) and len(pos) == 3 and isinstance(ori, (list, tuple)) and len(ori) == 4):
				rospy.logwarn('目标 {} 的 position 或 orientation 格式不正确'.format(key))
				continue
			seq.append({'name': key, 'position': pos, 'orientation': ori})
		return seq

	def _load_named_goal(self, yaml_path, goal_name):
		with open(yaml_path, 'r', encoding='utf-8') as f:
			data = yaml.safe_load(f) or {}
		goals_ns = data.get('goals', {})
		g = goals_ns.get(goal_name)
		if not g:
			return None
		pos = g.get('position', None)
		ori = g.get('orientation', None)
		if not (isinstance(pos, (list, tuple)) and len(pos) == 3 and isinstance(ori, (list, tuple)) and len(ori) == 4):
			rospy.logwarn('目标 {} 的 position 或 orientation 格式不正确'.format(goal_name))
			return None
		return {'name': goal_name, 'position': pos, 'orientation': ori}

	def _load_ball_goals(self, yaml_path):
		"""按顺序加载可用的 ball1..ball4 目标点。缺失的将被忽略。"""
		with open(yaml_path, 'r', encoding='utf-8') as f:
			data = yaml.safe_load(f) or {}
		goals_ns = data.get('goals', {})
		ball_names = ['ball1', 'ball2', 'ball3', 'ball4']
		balls = []
		for name in ball_names:
			g = goals_ns.get(name)
			if not g:
				continue
			pos = g.get('position', None)
			ori = g.get('orientation', None)
			if not (isinstance(pos, (list, tuple)) and len(pos) == 3 and isinstance(ori, (list, tuple)) and len(ori) == 4):
				rospy.logwarn('目标 {} 的 position 或 orientation 格式不正确'.format(name))
				continue
			balls.append({'name': name, 'position': pos, 'orientation': ori})
		return balls

	def _on_status(self, msg):
		# 不再依赖该回调判断成功
		return

	def _on_detect(self, msg):
		# 收到任意消息即视为识别成功
		self._detected.set()

	def _publish_goal(self, position, orientation):
		# 通过 Action 发送目标，以更一致地接收结果
		goal = MoveBaseGoal()
		goal.target_pose.header.stamp = rospy.Time.now()
		goal.target_pose.header.frame_id = self.frame_id
		goal.target_pose.pose.position.x = float(position[0])
		goal.target_pose.pose.position.y = float(position[1])
		goal.target_pose.pose.position.z = float(position[2])
		goal.target_pose.pose.orientation.x = float(orientation[0])
		goal.target_pose.pose.orientation.y = float(orientation[1])
		goal.target_pose.pose.orientation.z = float(orientation[2])
		goal.target_pose.pose.orientation.w = float(orientation[3])

		self.mb_client.send_goal(goal)
		self._last_goal_sent_time = rospy.Time.now()
		self._nav_succeeded.clear()
		rospy.loginfo('已下发导航目标 ({:.3f}, {:.3f}, {:.3f})'.format(
			goal.target_pose.pose.position.x, goal.target_pose.pose.position.y, goal.target_pose.pose.position.z))

	def _wait_for_nav_success(self):
		if self.nav_timeout_sec <= 0:
			self.mb_client.wait_for_result()
		else:
			finished = self.mb_client.wait_for_result(rospy.Duration(self.nav_timeout_sec))
			if not finished:
				self.mb_client.cancel_goal()
				rospy.logwarn('导航等待超时 ({:.1f}s)，已取消目标'.format(self.nav_timeout_sec))
				return False
		state = self.mb_client.get_state()
		# SUCCEEDED == 3
		if state == 3:
			return True
		else:
			rospy.logwarn('导航未成功，状态码: {}'.format(state))
			return False

	def _trigger_yolo_and_wait(self):
		# 触发识别
		self._detected.clear()
		self.yolo_pub.publish(String(data='yolostart'))
		rospy.loginfo('已发布识别触发: yolostart 到 /yolo_detect')
		# 等待识别完成
		start = time.time()
		rate = rospy.Rate(20)
		while not rospy.is_shutdown():
			if self._detected.is_set():
				return True
			if self.detect_timeout_sec > 0 and (time.time() - start) > self.detect_timeout_sec:
				rospy.logwarn('识别等待超时 ({:.1f}s)'.format(self.detect_timeout_sec))
				return False
			rate.sleep()

	def _backoff_before_box1(self, speed_mps=1, duration_sec=1.0):
		"""在下发 box1 前，先沿 x 轴后退指定时间。

		参数:
		- speed_mps: 后退速度，单位 m/s（正数表示速度幅值，内部会取负号）
		- duration_sec: 持续时间，单位 s
		"""
		backward_twist = Twist()
		backward_twist.linear.x = -abs(float(speed_mps))
		zero_twist = Twist()
		rospy.loginfo('在下发 box1 之前，先后退 {:.2f} m/s 持续 {:.1f}s'.format(abs(float(speed_mps)), float(duration_sec)))
		start_time = time.time()
		rate = rospy.Rate(20)
		while not rospy.is_shutdown() and (time.time() - start_time) < float(duration_sec):
			self.cmd_vel_pub.publish(backward_twist)
			rate.sleep()
		# 停止
		self.cmd_vel_pub.publish(zero_twist)
		rospy.sleep(0.1)

	def _strafe_right_before_box1(self, speed_mps=0.5, duration_sec=0.5):
		"""在下发 box1 前，后退完成后再向右平移指定时间。

		坐标约定：前方为 +x，左侧为 +y，因此右移为 -y。

		参数:
		- speed_mps: 右移速度，单位 m/s（正数表示速度幅值，内部会取负号）
		- duration_sec: 持续时间，单位 s
		"""
		right_twist = Twist()
		right_twist.linear.y = -abs(float(speed_mps))
		zero_twist = Twist()
		rospy.loginfo('在下发 box1 之前，先右移 {:.2f} m/s 持续 {:.1f}s'.format(abs(float(speed_mps)), float(duration_sec)))
		start_time = time.time()
		rate = rospy.Rate(20)
		while not rospy.is_shutdown() and (time.time() - start_time) < float(duration_sec):
			self.cmd_vel_pub.publish(right_twist)
			rate.sleep()
		# 停止
		self.cmd_vel_pub.publish(zero_twist)
		rospy.sleep(0.1)

	def run(self):
		for idx, g in enumerate(self.goals, start=1):
			if rospy.is_shutdown():
				break
			name = g['name']
			rospy.loginfo('开始导航到第 {} 个目标: {}'.format(idx, name))
			self._publish_goal(g['position'], g['orientation'])
			succeeded = self._wait_for_nav_success()
			if succeeded:
				_ = self._trigger_yolo_and_wait()
			else:
				rospy.logwarn('导航未成功，跳过识别，进入下一个目标')
			rospy.loginfo('完成第 {} 个目标处理: {}'.format(idx, name))

		# After completing 'six', optionally traverse ball1..ball4, then navigate to 'box1'
		if any(g['name'] == 'six' for g in self.goals):
			if self.ball_goals:
				rospy.loginfo("已完成'six'，开始遍历 ball1..ball4 目标点")
				# 在 six -> ball1 之间后退与右移（0.5 m/s，0.8s）
				self._backoff_before_box1(speed_mps=0.5, duration_sec=0.8)
				self._strafe_right_before_box1(speed_mps=0.5, duration_sec=0.8)
				for b in self.ball_goals:
					self._publish_goal(b['position'], b['orientation'])
					_ = self._wait_for_nav_success()
					rospy.loginfo('完成 ball 目标: {}'.format(b['name']))
			# 完成 ball* 后，直接追加导航到 'box1'（不进行后退/右移）
			if self.box1_goal:
				rospy.loginfo("追加导航到'box1'（不触发识别）")
				self._publish_goal(self.box1_goal['position'], self.box1_goal['orientation'])
				box1_ok = self._wait_for_nav_success()
				if box1_ok:
					rospy.loginfo("到达 box1，开始在 10.0s 内以 5Hz 发布 /nx_nano/start_nav=True")
					start_time = time.time()
					rate = rospy.Rate(5)
					msg = Bool(data=True)
					while not rospy.is_shutdown() and (time.time() - start_time) < 10.0:
						self.start_nav_pub.publish(msg)
						rospy.loginfo("已向 /nx_nano/start_nav 发布 True 触发信号")
						rate.sleep()
					rospy.loginfo("/nx_nano/start_nav 有限时发布结束")

		rospy.loginfo('所有目标处理完成')


def main():
	try:
		node = SequentialNavigator()
		node.run()
	except rospy.ROSInitException:
		pass
	except Exception as e:
		rospy.logerr('异常: {}'.format(e))


if __name__ == '__main__':
	main()


